﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems.CollisionPrimitives
{
    public class Polygon_2D : Polygon
    {
        public Polygon_2D(string rawFile)
            :base(rawFile)
        {

        }
        public void Set_Position(double x, double y)
        {
            Coordinates[0] = x;
            Coordinates[1] = y;
        }
        public void Set_Orientation(double theta)
        {
            Coordinates[2] = theta;
        }
        public double Get_X()
        {
            return Coordinates[0];
        }
        public double Get_Y()
        {
            return Coordinates[1];
        }
        public double Get_Theta()
        {
            return Coordinates[2];
        }

        private double Distance(double[] A, double[] B)
        {
            double dx = A[0] - B[0];
            double dy = A[1] - B[1];
            return Math.Sqrt(dx*dx + dy*dy);
        }
        private double Calculate_T(double[] A, double[] B, double[] C)
        {
            double length_AB = Distance(A, B);
            double x_AB = B[0] - A[0];
            double y_AB = B[1] - A[1];
            double x_AC = C[0] - A[0];
            double y_AC = C[1] - A[1];

            double proj_AC_on_AB = (x_AB * x_AC + y_AB * y_AC) / length_AB;

            return proj_AC_on_AB / length_AB;

        }

        override public bool IsCollidedWithIfAt(double[] coord, Polygon targetPolygon)
        {
            if (coord == null)
                coord = Coordinates;
            List<double[]> vertices_host = null;
            TransformToEnvironment(coord, ref vertices_host);
            List<double[]> vertices_target = null;
            targetPolygon.TransformToEnvironment(null, ref vertices_target);

            #region examine projections of host polygon
            int V_count = vertices_host.Count;
            for (int i = 0; i < V_count; ++i)
            {
                double min_t = 0;
                double max_t = 1;

                //find t range of host object
                //   locate A
                double[] A = vertices_host[i];
                //   locate B
                int i_next = i + 1;
                if (i_next == vertices_host.Count) i_next = 0;
                double[] B = vertices_host[i_next];
                //   locate C on each vertices of host polygon
                int i_cur = i_next + 1;
                for (int j = 2; j < V_count; ++j, i_cur++)
                {
                    int i_value = i_cur;
                    if (i_cur >= V_count)
                        i_value -= V_count;
                    double[] C = vertices_host[i_value];
                    double t_value = Calculate_T(A, B, C);
                    if (t_value < min_t) min_t = t_value;
                    if (t_value > max_t) max_t = t_value;
                }
                //check on each vertices of target polygon
                int n_above = 0;
                int n_below = 0;
                for (int j = 0; j < vertices_target.Count; j++)
                {
                    double[] C = vertices_target[j];
                    double t_value = Calculate_T(A, B, C);
                    if (t_value >= max_t)
                        n_above++;
                    else if (t_value <= min_t)
                        n_below++;
                    else
                        break;
                }
                if (n_above == vertices_target.Count)
                    return false;
                if (n_below == vertices_target.Count)
                    return false;
            }
            #endregion

            #region examine projections on target polygon
            V_count = vertices_target.Count;
            for (int i = 0; i < V_count; ++i)
            {
                double min_t = 0;
                double max_t = 1;

                //find t range of target object
                //   locate A
                double[] A = vertices_target[i];
                //   locate B
                int i_next = i + 1;
                if (i_next == vertices_target.Count) i_next = 0;
                double[] B = vertices_target[i_next];
                //   locate C on each vertices of target polygon
                int i_cur = i_next + 1;
                for (int j = 2; j < V_count; ++j, i_cur++)
                {
                    int i_value = i_cur;
                    if (i_cur >= V_count)
                        i_value -= V_count;
                    double[] C = vertices_target[i_value];
                    double t_value = Calculate_T(A, B, C);
                    if (t_value < min_t) min_t = t_value;
                    if (t_value > max_t) max_t = t_value;
                }
                //check on each vertices of host polygon
                int n_above = 0;
                int n_below = 0;
                for (int j = 0; j < vertices_host.Count; j++)
                {
                    double[] C = vertices_host[j];
                    double t_value = Calculate_T(A, B, C);
                    if (t_value >= max_t)
                        n_above++;
                    else if (t_value <= min_t)
                        n_below++;
                    else
                        break;
                }
                if (n_above == vertices_host.Count)
                    return false;
                if (n_below == vertices_host.Count)
                    return false;
            }




            #endregion

            return true;
        }

        override public bool IsCollidedWith(Polygon targetPolygon)
        {

            return IsCollidedWithIfAt(null, targetPolygon);
        }

        override public void TransformToEnvironment(double[] coord, ref List<double[]> vertices_real)
        {
            if (coord == null)
                coord = Coordinates;
            if (vertices_real == null)
                vertices_real = new List<double[]>(Vertices.Count);
            while (vertices_real.Count < Vertices.Count)
                vertices_real.Add(new double[2]);

            double theta = coord[2];
            double M11 = Math.Cos(theta);
            double M21 = Math.Sin(theta);
            double M12 = -M21;
            double M22 = M11;

            for (int i = 0; i < Vertices.Count; ++i)
            {
                double x = Vertices[i][0];
                double y = Vertices[i][1];
                double[] n_coord = new double[2];
                vertices_real[i][0] = coord[0] + M11 * x + M12 * y;
                vertices_real[i][1] = coord[1] + M21 * x + M22 * y;
            }
        }
        
    }
}
